/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */

/*
 * IMPORTAINT: Drive configuration:
 * Top Left = 1
 * Top Right = 2
 * Bottom Left = 3
 * Bottom Right = 4
 * 
 * Declarations of variables are listed below: Joysticks, drive loadout, 
 * compressor, and the solenoids.
 */
public class RobotTemplate extends SimpleRobot {
    RobotDrive drive = new RobotDrive(1, 3, 2, 4);
    // Joystick leftStick = new Joystick(1);     
    // Joystick rightStick = new Joystick(2);
    LogitechController controller = new LogitechController(1); 

    Compressor compressor = new Compressor(9, 1); 
    Solenoid pistonUp = new Solenoid(1); //Plus is forward / up
    Solenoid pistonDown = new Solenoid(2); // Minus is backward / down
    Gyro gyro = new Gyro(2); 
    
    double leftOffset = 0;
    double rightOffset = 0;
    
    public RobotTemplate() {
        compressor.start();
    }
    
    public void disabled()
    {
        System.out.println("gyro = " + gyro.getAngle());        
    }
    
    /*
    static final int joystickButtonCount = 22;
    boolean[] rightStickButtons = new boolean[ (joystickButtonCount + 1) ];
    boolean[] leftStickButtons = new boolean[ (joystickButtonCount + 1) ];
    
    static final int solenoidNumber = 2;
    Solenoid[] numberOfSolenoids = new Solenoid[solenoidNumber];
    
    public RobotTemplate() {
    
    // Iterate over all the buttons on each joystick, setting state to false for each
    int buttonNum = 1;						// start counting buttons at button 1
    for (buttonNum = 1; buttonNum <= joystickButtonCount; buttonNum++) {
            rightStickButtons[buttonNum] = false;
            leftStickButtons[buttonNum] = false;
    }

    // Iterate over all the solenoids on the robot, constructing each in turn
    int solenoidNum = 1;						// start counting solenoids at solenoid 1
    for (solenoidNum = 0; solenoidNum < solenoidNumber; solenoidNum++) {
            numberOfSolenoids[solenoidNum] = new Solenoid(solenoidNum + 1);
         }
    } // digital in 9, relay 1
    */
    
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        for (int i = 0; i < 4; i++) { 
        drive.drive(0.5, 0.0);  // drive 50% forward speed with 0% turn 
        Timer.delay(2.0);   // wait 2 seconds 
        drive.drive(0.0, 0.75); // drive 0% forward with 75% turn 
        Timer.delay(0.75);  // wait for the 90 degree turn to complete 
        } 
        drive.drive(0.0, 0.0);   // drive 0% forward with 0% turn (stop) 
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        { 
            //getWatchdog().setEnabled(true);
            while (isOperatorControl() && isEnabled())  // loop during enabled teleop mode 
            { 
                
                if(controller.getButtonStart())
                {
                    leftOffset = controller.getLeftY();
                    rightOffset = controller.getRightY();
                    System.out.println("Left Offset = " + leftOffset + " Right Offset = " + rightOffset);
                }
                
                //getWatchdog().feed();
                //drive.tankDrive(leftStick, rightStick);  // drive with the joysticks 
                drive.tankDrive(controller.getLeftY()-leftOffset, controller.getRightY()-rightOffset);
                Timer.delay(0.005);
                
                System.out.println("gyro = " + gyro.getAngle());
                /*
                 * The following code allows for the operator to control
                 * the pneumatics system with the left trigger extending the 
                 * piston and the second button on the left joystick retracting
                 * the piston. Recall that the solenoids are in ports 1 and 2,
                 * respectively.
                 */
                if (controller.getButtonA()) {
                    pistonUp.set(false);                    
                    pistonDown.set(true);
                } else if (controller.getButtonB()) {
                    pistonUp.set(true);
                    pistonDown.set(false);
                } else {
                    pistonUp.set(false);
                    pistonDown.set(false);
                }
                

            }
        }
    }
}
